//
// Created by ms on 2025/10/21.
//

#include "LowpassFilter.h"

#include <chrono>


float LowpassFilter::operator()(float input) {
	auto timestamp = get_micros();

	float dt = (float)(timestamp - last_timestamp) * 1e-6f;

	if (dt < 0.0f) {
		dt = 1e-3f;
	}
	else if (dt > 0.3f) {
		//300ms
		last_output    = input;
		last_timestamp = timestamp;
		return input;
	}

	if (_Tf == 0) return input;

	float alpha = _Tf / (_Tf + dt);
	float output = alpha * last_output + (1.0f - alpha) * input;
	last_output = output;
	last_timestamp = timestamp;

	return output;
}
